#include <ros/ros.h>
#include <ros/serialization.h>

#include "cg_interfaces/ChassisStruct.h"
#include "cg_remote_control/udp_backend.hpp"

const ssize_t BufferSize = 1024;
unsigned char rec_buffer[BufferSize];
unsigned char serialize_buffer[BufferSize];
ros::Subscriber sub;
ros::Publisher pub;
CG::NetEndPoint boardCast;
CG::UDPBackEnd *backend;
bool needInit = false;

void OnChassisStructReceiveFromRC(const cg_interfaces::ChassisStruct::ConstPtr &msg)
{
    ROS_INFO("bridge_changed: chassis changed by remote control");
    pub.publish(msg);
    int length = ros::serialization::serializationLength(*msg);
    ros::serialization::OStream ostream(serialize_buffer, length);
    ros::serialization::serialize(ostream, *msg);
    backend->Send(serialize_buffer, length, boardCast);
}

bool TryGetParams(std::string &ip, int &port, cg_interfaces::ChassisStruct &init)
{
    ros::NodeHandle nh("~");
    if (!nh.getParam("ip", ip))
    {
        ROS_ERROR("Failed to get ip parameter");
        return false;
    }
    if (!nh.getParam("port", port))
    {
        ROS_ERROR("Failed to get port parameter");
        return false;
    }
    std::string init_str;
    if (nh.getParam("init_struct", init_str))
    {
        int index = init_str.find_first_of(':');
        init.master = std::stoi(init_str.substr(0, index));
        int count = 0;
        while (true)
        {
            int index2 = init_str.find_first_of(',', index + 1);
            if (index2 == -1)
            {
                break;
            }
            init.struct_ids[count] = std::stoi(init_str.substr(index + 1, index2 - index - 1));
            count++;
            index = index2;
        }
        init.struct_ids[count] = std::stoi(init_str.substr(index + 1));
        init.count = count + 1;
    }
    else
    {
        init.count = 0;
    }
    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "chassis_manager");
    std::string ip;
    int port;
    cg_interfaces::ChassisStruct init;
    if (!TryGetParams(ip, port, init))
    {
        ROS_ERROR("Failed to get parameters");
        ros::shutdown();
        return -1;
    }
    if (init.count == 0)
    {
        needInit = false;
    }

    boardCast = CG::NetEndPoint(ip, port);
    backend = new CG::UDPBackEnd(CG::NetEndPoint(port));
    ros::NodeHandle nh;
    sub = nh.subscribe<cg_interfaces::ChassisStruct>("chassis_struct_rc", 1, OnChassisStructReceiveFromRC);
    pub = nh.advertise<cg_interfaces::ChassisStruct>("chassis_struct", 1);

    ssize_t length = 0;
    CG::NetEndPoint remote_endpoint;
    ros::Rate rate(10);
    while (ros::ok())
    {
        if (needInit == true)
        {
            ros::spinOnce();
            pub.publish(init);
            needInit = false;
            ROS_INFO("%d:%d,%d,%d,%d", init.master, init.struct_ids[0], init.struct_ids[1], init.struct_ids[2],
                     init.struct_ids[3]);
        }
        if (backend->Receive(rec_buffer, BufferSize, length, remote_endpoint))
        {
            cg_interfaces::ChassisStruct msg;
            ros::serialization::IStream istr(rec_buffer, length);
            ros::serialization::deserialize(istr, msg);
            pub.publish(msg);
        }
        ros::spinOnce();
        rate.sleep();
    }
    if (backend != nullptr)
    {
        delete backend;
    }
    return 0;
}